#include <stdio.h>
#include <string.h>
#include "bridge_motors.h"

static int enable_chissp(void *self);

struct device_pwm_i device_pwm = {
    .pwm_init = (device_pwm_init_fn_t)device_pwm_init,
    .pwm_set = (device_pwm_set_fn_t)device_pwm_set,
    .pwm_deinit = (device_pwm_deinit_fn_t)device_pwm_deinit,
};
struct device_hb_gpio_i device_hb_gpio = {
    .gpio_hb_init = (device_hb_gpio_init_fn_t)device_hb_gpio_init,
    .gpio_hb_set = (device_hb_gpio_set_fn_t)device_hb_gpio_set,
};

struct bridge_motors_i bm_interface = {
    .correct = (bridge_motors_correct_fn_t)motors_correct,
    .inverse = (bridge_motors_inverse_fn_t)motors_inverse,
    .hasten = (bridge_motors_hasten_fn_t)motors_hasten,
    .decelerate = (bridge_motors_decelerate_fn_t)motors_decelerate,
    .stop = (bridge_motors_stop_fn_t)motors_stop,
};

/*H桥电机驱动初始化*/
int bridge_motors_init(struct bridge_motors *self,struct bridge_motors_config *cfg)
{
    self->config = cfg;
    memset(self->io_list,0,sizeof(self->io_list)); // 数组整体初始化为0
    self->io_list[0] = cfg->io_sd1;
    self->io_list[1] = cfg->io_sd2;
    self->interface = &bm_interface;
    self->device_pwm = &device_pwm;
    self->device_hb_gpio = &device_hb_gpio;
    self->device_pwm->pwm_init(self->config->pwm_hz,1,self->config->pwm_io1);
    self->device_pwm->pwm_init(self->config->pwm_hz,2,self->config->pwm_io2);
    self->device_hb_gpio->gpio_hb_init(self->io_list);
    self->enable_chips = enable_chissp;
    self->enable_chips(self);
    self->direction = 1;                    // 1正转，0反转
    return 0;
}

/*正转*/
int motors_correct(struct bridge_motors *self)
{
    self->direction = 1;
    self->device_pwm->pwm_set(1,self->config->pwm_duty);
    self->device_pwm->pwm_set(2,self->config->pwm_min_duty);
    return 0;
}
/*反转*/
int motors_inverse(struct bridge_motors *self)
{
    self->direction = 0;
    self->device_pwm->pwm_set(1,self->config->pwm_min_duty);
    self->device_pwm->pwm_set(2,self->config->pwm_duty);
    return 0;
}
/*加速*/
int motors_hasten(struct bridge_motors *self)
{
    if(self->config->pwm_duty >= self->config->pwm_max_duty)
    {
        self->config->pwm_duty = self->config->pwm_max_duty;
        return 1;
    }
    else
    {
        self->config->pwm_duty += self->config->pwm_add_duty;
    }

    if(self->direction == 1)
    {
        self->device_pwm->pwm_set(1,self->config->pwm_duty);
        self->device_pwm->pwm_set(2,self->config->pwm_min_duty);
    }else{
        self->device_pwm->pwm_set(1,self->config->pwm_min_duty);
        self->device_pwm->pwm_set(2,self->config->pwm_duty);
    }
    return 0;
}
/*减速*/
int motors_decelerate(struct bridge_motors *self)
{
    if(self->config->pwm_duty <= self->config->pwm_min_duty)
    {
        self->config->pwm_duty = self->config->pwm_min_duty;
        return 1;
    }
    else
    {
        self->config->pwm_duty -= self->config->pwm_add_duty;
    }
    if(self->direction == 1)
    {
        self->device_pwm->pwm_set(1,self->config->pwm_duty);
        self->device_pwm->pwm_set(2,self->config->pwm_min_duty);
    }else{
        self->device_pwm->pwm_set(1,self->config->pwm_min_duty);
        self->device_pwm->pwm_set(2,self->config->pwm_duty);
    }
    return 0;
}
/*停止*/
int motors_stop(struct bridge_motors *self)
{
    self->device_pwm->pwm_set(1,self->config->pwm_min_duty);
    self->device_pwm->pwm_set(2,self->config->pwm_min_duty);
    printf("stop\n");
    return 0;
}

static int enable_chissp(void *self)
{
    struct bridge_motors *chip = (struct bridge_motors *)self;
    chip->device_hb_gpio->gpio_hb_set(chip->io_list[0],0);
    chip->device_hb_gpio->gpio_hb_set(chip->io_list[1],0);
    chip->device_hb_gpio->gpio_hb_set(chip->io_list[0],1);
    chip->device_hb_gpio->gpio_hb_set(chip->io_list[1],1);
    return 0;
}